#include "constants.h"
#include "library.h"
#include "PathSrv.h"
#include "TrajectoryPlanner.h"
#include "MapTransform.h"
#include "fnavfn/Path.h"
#include "ObstacleDetector.h"

#ifndef CONTROLLER_H_
#define CONTROLLER_H_

class Controller {
	double x,y,t,lv,av,mx,my,THRESHOLD;
	boost::thread thr;
	double modx, mody, modt, goalx, goaly;
	ObstacleDetector detector;


	ros::Publisher pub;
	ros::Subscriber sub;
	ros::ServiceServer control_service;

	tf::TransformListener listener;
	tf::StampedTransform transform;

	TrajectoryPlanner tp;
	MapTransform mapper;
	void get_map_odom_transform();
	void get_erratic_pose(nav_msgs::Odometry msg);
	bool goal_reached();

public:
	Controller(ros::NodeHandle &n);
	virtual ~Controller();
	bool control(fnavfn::Path::Request &req, fnavfn::Path::Response &res );
};

#endif /* CONTROLLER_H_ */
